﻿using System;
using System.Text;
using System.Drawing;
using System.Buffers;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;

public static partial class NativeAOT
{
    [UnmanagedCallersOnly(EntryPoint = "gear")]
    public static unsafe int gear(double a, double b, double hmin, double hmax, double h, double eps, int n, IntPtr y0_ptr, int k, IntPtr t_ptr, IntPtr z_ptr, IntPtr ss_x_ya_n_da_ptr, IntPtr f_x_ya_n_da_ptr)
    {
        double* y0 = (double*)y0_ptr.ToPointer();
        double* t = (double*)t_ptr.ToPointer();
        double* z = (double*)z_ptr.ToPointer();
        ss_x_ya_n_da = Marshal.GetDelegateForFunctionPointer<delegatefunc_x_ya_n_da>(ss_x_ya_n_da_ptr);
        f_x_ya_n_da = Marshal.GetDelegateForFunctionPointer<delegatefunc_x_ya_n_da>(f_x_ya_n_da_ptr);

        return gear(a, b, hmin, hmax, h, eps, n, y0, k, t, z);
    }

    /// <summary>
    /// 积分刚性方程组的Gear方法
    /// ss 计算雅可比矩阵的函数名。
    /// f 计算方程组中各方程右端函数值的函数名。
    /// </summary>
    /// <param name="a">积分区间起始点。</param>
    /// <param name="b">积分区间终点。</param>
    /// <param name="hmin">积分过程中允许的最小步长。</param>
    /// <param name="hmax">积分过程中允许的最大步长。</param>
    /// <param name="h">积分的拟定步长。hmin<<h<hmax。</param>
    /// <param name="eps">误差检验常数。</param>
    /// <param name="n">方程个数，也是未知数个数。</param>
    /// <param name="y0">y0[n]n个未知函数在起始点处的函数值。</param>
    /// <param name="k">拟定输出的积分点数。</param>
    /// <param name="t">t[k]返回实际有效输出点（包括起始点）的自变量值。</param>
    /// <param name="z">z[n][k]返回实际有效输出点处的未知函数值。</param>
    /// <returns>
    /// 函数返回实际输出的积分点数。在函数返回之前，会给出下列相应信息供参考:
    /// 全区间积分成功。若此时输出点数不够，可增大积分区间终点值
    /// 步长小于hmin，精度达不到，积分停止(前输出点有效)
    /// 阶数已大于6，积分停止(前输出点有效)
    /// 对于h>hmin校正迭代不收敛，积分停止(前输出点有效)
    /// 精度要求太高，积分停止(前输出点有效)
    /// </returns>
    public static unsafe int gear(double a, double b, double hmin, double hmax, double h, double eps, int n, double* y0, int k, double* t, double* z)
    {
        int kf, jt, nn, nq, i, m, irt1, irt, j, nqd, idb = 0;
        int iw = 0, j1, j2, nt, nqw, l;
        double hw, hd, rm, t0, td, r = 0.0, dd, pr1, pr2, pr3, rr;
        double enq1 = 0.0, enq2 = 0.0, enq3 = 0.0, eup = 0.0, e = 0.0, edwn = 0.0, bnd = 0.0, r1;
        double[,] pp = new double[7, 3] {
            { 2.0,3.0,1.0}, { 4.5,6.0,1.0},
            { 7.333,9.167,0.5}, { 10.42,12.5,0.1667},
            { 13.7,15.98,0.04133},  { 17.15,1.0,0.008267},
            { 1.0,1.0,1.0}
        };
        double* aa = stackalloc double[7];
        double* d = stackalloc double[n];
        double* p = stackalloc double[n * n];
        double* s = stackalloc double[10 * n];
        double* s02 = stackalloc double[n];
        double* ym = stackalloc double[n];
        double* er = stackalloc double[n];
        double* yy = stackalloc double[n];
        double* y = stackalloc double[8 * n];

        aa[1] = -1.0;
        jt = 0;
        nn = 0;
        nq = 1;
        t0 = a;
        for (i = 0; i <= 8 * n - 1; i++)
        {
            y[i] = 0.0;
        }
        for (i = 0; i <= n - 1; i++)
        {
            y[i * 8] = y0[i];
            yy[i] = y[i * 8];
        }
        f_x_ya_n_da(t0, yy, n, d);
        for (i = 0; i <= n - 1; i++)
        {
            y[i * 8 + 1] = h * d[i];
        }
        hw = h;
        m = 2;
        for (i = 0; i <= n - 1; i++)
        {
            ym[i] = 1.0;
        }

    l20:

        irt = 1;
        kf = 1;
        nn = nn + 1;
        t[nn - 1] = t0;
        for (i = 0; i <= n - 1; i++)
        {
            z[i * k + nn - 1] = y[i * 8];
        }
        // 全区间积分成功
        if ((t0 >= b) || (nn == k))
        {
            return (nn);
        }
        for (i = 0; i <= n - 1; i++)
        {
            for (j = 0; j <= m - 1; j++)
            {
                s[i * 10 + j] = y[i * 8 + j];
            }
        }
        hd = hw;
        if (h != hd)
        {
            rm = h / hd;
            irt1 = 0;
            rr = Math.Abs(hmin / hd);
            if (rm < rr) rm = rr;
            rr = Math.Abs(hmax / hd);
            if (rm > rr) rm = rr;
            r = 1.0;
            irt1 = irt1 + 1;
            for (j = 1; j <= m - 1; j++)
            {
                r = r * rm;
                for (i = 0; i <= n - 1; i++)
                {
                    y[i * 8 + j] = s[i * 10 + j] * r;
                }
            }
            h = hd * rm;
            for (i = 0; i <= n - 1; i++)
            {
                y[i * 8] = s[i * 10];
            }
            idb = m;
        }
        nqd = nq;
        td = t0;
        rm = 1.0;
        if (jt > 0)
        {
            goto l80;
        }

    l60:

        switch (nq)
        {
            case 1:
                aa[0] = -1.0;
                break;
            case 2:
                aa[0] = -2.0 / 3.0;
                aa[2] = -1.0 / 3.0;
                break;
            case 3:
                aa[0] = -6.0 / 11.0;
                aa[2] = aa[0];
                aa[3] = -1.0 / 11.0;
                break;
            case 4:
                aa[0] = -0.48;
                aa[2] = -0.7;
                aa[3] = -0.2;
                aa[4] = -0.02;
                break;
            case 5:
                aa[0] = -120.0 / 274.0;
                aa[2] = -225.0 / 274.0;
                aa[3] = -85.0 / 274.0;
                aa[4] = -15.0 / 274.0;
                aa[5] = -1.0 / 274.0;
                break;
            case 6:
                aa[0] = -720.0 / 1764.0;
                aa[2] = -1624.0 / 1764.0;
                aa[3] = -735.0 / 1764.0;
                aa[4] = -175.0 / 1764.0;
                aa[5] = -21.0 / 1764.0;
                aa[6] = -1.0 / 1764.0;
                break;
            default:
                //阶数大于6，积分停止
                return (nn);
        }
        m = nq + 1;
        idb = m;
        enq2 = 0.5 / (nq + 1.0);
        enq3 = 0.5 / (nq + 2.0);
        enq1 = 0.5 / (nq + 0.0);
        eup = pp[nq - 1, 1] * eps;
        eup = eup * eup;
        e = pp[nq - 1, 0] * eps;
        e = e * e;
        edwn = pp[nq - 1, 2] * eps;
        edwn = edwn * edwn;
        if (edwn == 0.0)
        {
            //精度要求太高，积分停止
            return (nn);
        }
        bnd = eps * enq3 / (n + 0.0);
        iw = 1;
        if (irt == 2)
        {
            r1 = 1.0;
            for (j = 1; j <= m - 1; j++)
            {
                r1 = r1 * r;
                for (i = 0; i <= n - 1; i++)
                {
                    y[i * 8 + j] = y[i * 8 + j] * r1;
                }
            }
            idb = m;
            for (i = 0; i <= n - 1; i++)
            {
                if (ym[i] < Math.Abs(y[i * 8]))
                {
                    ym[i] = Math.Abs(y[i * 8]);
                }
            }
            jt = nq;
            goto l20;
        }

    l80:

        t0 = t0 + h;
        for (j = 2; j <= m; j++)
        {
            for (j1 = j; j1 <= m; j1++)
            {
                j2 = m - j1 + j - 1;
                for (i = 0; i <= n - 1; i++)
                {
                    y[i * 8 + j2 - 1] = y[i * 8 + j2 - 1] + y[i * 8 + j2];
                }
            }
        }
        for (i = 0; i <= n - 1; i++)
        {
            er[i] = 0.0;
        }
        j1 = 1;
        nt = 1;
        for (l = 0; l <= 2; l++)
        {
            if ((j1 != 0) && (nt != 0))
            {
                for (i = 0; i <= n - 1; i++)
                {
                    yy[i] = y[i * 8];
                }
                f_x_ya_n_da(t0, yy, n, d);
                if (iw >= 1)
                {
                    for (i = 0; i <= n - 1; i++)
                    {
                        yy[i] = y[i * 8];
                    }
                    ss_x_ya_n_da(t0, yy, n, p);
                    r = aa[0] * h;
                    for (i = 0; i <= n - 1; i++)
                    {
                        for (j = 0; j <= n - 1; j++)
                        {
                            p[i * n + j] = p[i * n + j] * r;
                        }
                    }
                    for (i = 0; i <= n - 1; i++)
                    {
                        p[i * n + i] = 1.0 + p[i * n + i];
                    }
                    iw = -1;
                    j1 = inv(p, n);
                }
                if (j1 != 0)
                {
                    for (i = 0; i <= n - 1; i++)
                    {
                        s02[i] = y[i * 8 + 1] - d[i] * h;
                    }
                    for (i = 0; i <= n - 1; i++)
                    {
                        dd = 0.0;
                        for (j = 0; j <= n - 1; j++)
                        {
                            dd = dd + s02[j] * p[i * n + j];
                        }
                        s[i * 10 + 8] = dd;
                    }
                    nt = n;
                    for (i = 0; i <= n - 1; i++)
                    {
                        y[i * 8] = y[i * 8] + aa[0] * s[i * 10 + 8];
                        y[i * 8 + 1] = y[i * 8 + 1] - s[i * 10 + 8];
                        er[i] = er[i] + s[i * 10 + 8];
                        if (Math.Abs(s[i * 10 + 8]) <= (bnd * ym[i]))
                        {
                            nt = nt - 1;
                        }
                    }
                }
            }
        }
        if (nt > 0)
        {
            t0 = td;
            if ((h > (hmin * 1.00001)) || (iw >= 0))
            {
                if (iw != 0) rm = 0.25 * rm;
                iw = 1; irt1 = 2;
                rr = Math.Abs(hmin / hd);
                if (rm < rr) rm = rr;
                rr = Math.Abs(hmax / hd);
                if (rm > rr) rm = rr;
                r = 1.0;
                for (j = 1; j <= m - 1; j++)
                {
                    r = r * rm;
                    for (i = 0; i <= n - 1; i++)
                    {
                        y[i * 8 + j] = s[i * 10 + j] * r;
                    }
                }
                h = hd * rm;
                for (i = 0; i <= n - 1; i++)
                {
                    y[i * 8] = s[i * 10];
                }
                idb = m;
                goto l80;
            }
            //h>=hmin校正迭代不收敛,积分停止
            return (nn);
        }
        dd = 0.0;
        for (i = 0; i <= n - 1; i++)
        {
            dd = dd + (er[i] / ym[i]) * (er[i] / ym[i]);
        }
        iw = 0;
        if (dd <= e)
        {
            if (m >= 3)
            {
                for (j = 2; j <= m - 1; j++)
                {
                    for (i = 0; i <= n - 1; i++)
                    {
                        y[i * 8 + j] = y[i * 8 + j] + aa[j] * er[i];
                    }
                }
            }
            kf = 1;
            hw = h;
            if (idb > 1)
            {
                idb = idb - 1;
                if (idb <= 1)
                {
                    for (i = 0; i <= n - 1; i++)
                    {
                        s[i * 10 + 9] = er[i];
                    }
                }
                for (i = 0; i <= n - 1; i++)
                {
                    if (ym[i] < Math.Abs(y[i * 8]))
                    {
                        ym[i] = Math.Abs(y[i * 8]);
                    }
                }
                jt = nq;
                goto l20;
            }
        }
        if (dd > e)
        {
            kf = kf - 2;
            if (h <= (hmin * 1.00001))
            {
                //步长已小于hmin，但精度达不到，积分停止
                return (nn);
            }
            t0 = td;
            if (kf <= -5)
            {
                if (nq == 1)
                {
                    //要求的精度太高,积分停止
                    return (nn);
                }
                for (i = 0; i <= n - 1; i++)
                {
                    yy[i] = y[i * 8];
                }
                f_x_ya_n_da(t0, yy, n, d);
                r = h / hd;
                for (i = 0; i <= n - 1; i++)
                {
                    y[i * 8] = s[i * 10];
                    s[i * 10 + 1] = hd * d[i];
                    y[i * 8 + 1] = s[i * 10 + 1] * r;
                }
                nq = 1;
                kf = 1;
                goto l60;
            }
        }
        pr2 = Math.Log(dd / e);
        pr2 = enq2 * pr2;
        pr2 = Math.Exp(pr2);
        pr2 = 1.2 * pr2;
        pr3 = 1.0e+20;
        if (nq < 7)
        {
            if (kf > -1)
            {
                dd = 0.0;
                for (i = 0; i <= n - 1; i++)
                {
                    pr3 = (er[i] - s[i * 10 + 9]) / ym[i];
                    dd = dd + pr3 * pr3;
                }
                pr3 = Math.Log(dd / eup);
                pr3 = enq3 * pr3;
                pr3 = Math.Exp(pr3);
                pr3 = 1.4 * pr3;
            }
        }
        pr1 = 1.0e+20;
        if (nq > 1)
        {
            dd = 0.0;
            for (i = 0; i <= n - 1; i++)
            {
                pr1 = y[i * 8 + m - 1] / ym[i];
                dd = dd + pr1 * pr1;
            }
            pr1 = Math.Log(dd / edwn); pr1 = enq1 * pr1;
            pr1 = Math.Exp(pr1); pr1 = 1.3 * pr1;
        }
        if (pr2 <= pr3)
        {
            if (pr2 > pr1)
            {
                r = 1.0e+04;
                if (pr1 > 1.0e-04) r = 1.0 / pr1;
                nqw = nq - 1;
            }
            else
            {
                nqw = nq; r = 1.0e+04;
                if (pr2 > 1.0e-04) r = 1.0 / pr2;
            }
        }
        else
        {
            if (pr3 < pr1)
            {
                r = 1.0e+04;
                if (pr3 > 1.0e-04) r = 1.0 / pr3;
                nqw = nq + 1;
            }
            else
            {
                r = 1.0e+04;
                if (pr1 > 1.0e-04) r = 1.0 / pr1;
                nqw = nq - 1;
            }
        }
        idb = 10;
        if (kf == 1)
        {
            if (r < 1.1)
            {
                for (i = 0; i <= n - 1; i++)
                {
                    if (ym[i] < Math.Abs(y[i * 8]))
                    {
                        ym[i] = Math.Abs(y[i * 8]);
                    }
                }
                jt = nq; goto l20;
            }
        }
        if (nqw > nq)
        {
            for (i = 0; i <= n - 1; i++)
            {
                y[i * 8 + nqw] = er[i] * aa[m - 1] / (m + 0.0);
            }
        }
        m = nqw + 1;
        if (kf == 1)
        {
            irt = 2;
            rr = hmax / Math.Abs(h);
            if (r > rr) r = rr;
            h = h * r;
            hw = h;
            if (nq == nqw)
            {
                r1 = 1.0;
                for (j = 1; j <= m - 1; j++)
                {
                    r1 = r1 * r;
                    for (i = 0; i <= n - 1; i++)
                    {
                        y[i * 8 + j] = y[i * 8 + j] * r1;
                    }
                }
                idb = m;
                for (i = 0; i <= n - 1; i++)
                {
                    if (ym[i] < Math.Abs(y[i * 8]))
                    {
                        ym[i] = Math.Abs(y[i * 8]);
                    }
                }
                jt = nq;
                goto l20;
            }
            nq = nqw;
            goto l60;
        }
        rm = rm * r;
        irt1 = 3;
        rr = Math.Abs(hmin / hd);
        if (rm < rr) rm = rr;
        rr = Math.Abs(hmax / hd);
        if (rm > rr) rm = rr;
        r = 1.0;
        for (j = 1; j <= m - 1; j++)
        {
            r = r * rm;
            for (i = 0; i <= n - 1; i++)
            {
                y[i * 8 + j] = s[i * 10 + j] * r;
            }
        }
        h = hd * rm;
        for (i = 0; i <= n - 1; i++)
        {
            y[i * 8] = s[i * 10];
        }
        idb = m;
        if (nqw == nq) goto l80;
        nq = nqw;
        goto l60;
    }

    /*
    // 积分刚性方程组的Gear方法例
      int main()
      { 
          int i,j,k,m;
          void  gearf(double,double [],int,double []);
          void  gears(double,double [],int,double []);
          double a,b,hmax,h,y[3],t[30],z[3][30];
          double hmin[4]={0.0001,0.0001,0.00001,0.00001};
          double eps[4]={0.0001,0.00001,0.00001,0.000001};
          a=0.0; b=1.0; h=0.01; hmax=0.1;
          for (k=0; k<=3; k++)
          { 
              y[0]=1.0; y[1]=0.0; y[2]=-1.0;
              m=gear(a,b,hmin[k],hmax,h,eps[k],3,y,30,t,&z[0][0],gears,gearf);
              cout <<"h = " <<h <<endl;
              cout <<"hmin = " <<hmin[k] <<endl;
              cout <<"hmax = " <<hmax <<endl;
              cout <<"eps = " <<eps[k] <<endl;
              for (i=0; i<m; i++)
              { 
                  cout <<"t(" <<setw(2) <<i <<")=" <<setw(13) <<t[i];
                  for (j=0; j<=2; j++) 
                      cout <<" y(" <<j <<")=" <<setw(13) <<z[j][i];
                  cout <<endl;
              }
              cout <<endl;
          }
          return 0;
      }
    // 计算方程组各方程右端函数值
      void gearf(double t, double y[], int n, double d[])
      { 
             //消除编译时的警告信息
    t=t; n=n;
          d[0]=-21.0*y[0]+19.0*y[1]-20.0*y[2];
          d[1]=19.0*y[0]-21.0*y[1]+20.0*y[2];
          d[2]=40.0*y[0]-40.0*y[1]-40.0*y[2];
          return;
      }
    // 计算雅可比矩阵
      void gears(double t, double y[], int n, double p[])
      { 
             //消除编译时的警告信息
    t=t;  y[0]=y[0];
          p[0*n+0]=-21.0;   p[0*n+1]=19.0;      p[0*n+2]=-20.0;
          p[1*n+0]=19.0;    p[1*n+1]=-21.0;     p[1*n+2]=20.0;
          p[2*n+0]=40.0;    p[2*n+1]=-40.0;     p[2*n+2]=-40.0;
          return;
      }
    */
}

